// This program demonstrates how to use the built-in features of the minibot and its software library.

#include <Protobot.h>  // the Protobot library contains the functions made specifically for this minibot.

Protobot robot;  // create a Protobot object se we can use its functions

void setup()  // this runs once after reset
{
  robot.begin();  // sets up the hardware connections
  Serial.begin(9600);  // start USB serial interface 
  delay(3000); // wait 3 seconds
  Serial.println("hello, world!"); // say hello to anyone who is listening
}

void loop()  // this normally repeats itself until power off
{
  robot.forward(255);  // go forward at full speed...
  delay(2000);  // for one second
  robot.stop();  // then stop
  delay(1000);  // wait half a second
  robot.turnLeft(200);  //  spin to the left at 80% speed...
  delay(1500);  // for 750 ms
  robot.turnRight(200);  // spin back to the right...
  delay(1500);  // for 750 ms
  robot.stop();  // and stop
  delay(1000);
  robot.reverse(255);  // reverse at full speed...
  delay(1000); // for 500 ms
  robot.stop();
  
  delay(1000);
  robot.leftMotor(-150);  // we can set each motor seperately
  robot.rightMotor(-255);  // this will make us reverse while spinning right
  delay(2000);
  robot.stop();
  
  delay(1000);
  robot.turnServo(-30);  //  point the servo 45 degrees left of center
  delay(1000);
  robot.turnServo(60);  // point the servo 60 degrees right of center (+/- 60 is maximum)
  delay(1000);
  robot.turnServo(0);  // point the servo straight ahead
  delay(1000);
  
  int sensorReading = robot.readSensor();  // read the distance sensor
  Serial.println(sensorReading);  // send the result over the USB connection
  int numBlinks = (800 - sensorReading) / 100;  // we will blink the LED to indicate distance
  for(int i = 0; i < numBlinks; i++)
  {
    digitalWrite(13,HIGH);  // turn LED on
    delay(300);
    digitalWrite(13,LOW);  // turn LED off
    delay(300);
  }
  while(1);  // do nothing forever (so we do not repeat)
}
